#include "stm32f10x.h"
#include "Delay.h"
#include "IIC.h"
#include "IIC2.h"
#include "Action.h"
#include "stdio.h"

# define Slave_Addr 0x36<<1
# define Angle_Hight_Register_Addr 0x0C
# define Angle_Low_Register_Addr 0x0D

 
float Get_Angle1(void)
{
	u8 buf[2] = {0};
	float temp = 0;
	
	buf[0]=IIC_Read_OneAddress(Slave_Addr,Angle_Hight_Register_Addr);
	buf[1]=IIC_Read_OneAddress(Slave_Addr,Angle_Low_Register_Addr);
	
	temp = (((u16)buf[0]<< 8) | buf[1]);
	temp=temp/4096*360;
  return temp;
}

float Encoder1_Action=0;

void Encoder1_update(void)
{
	static float encoder_Before=0;
	static float encoder_Now=0;
	float Different=0;
	encoder_Now=Get_Angle1();
	if(encoder_Now-encoder_Before<-200) Different=encoder_Now+360.0f-encoder_Before;
  else if(encoder_Now-encoder_Before>200) Different=encoder_Now-360.0f-encoder_Before;
	else Different=encoder_Now-encoder_Before;
	
	Encoder1_Action+=Different;
	encoder_Before=encoder_Now;
}




float Get_Angle2(void)
{
	u8 buf[2] = {0};
	float temp = 0;
	
	buf[0]=IIC2_Read_OneAddress(Slave_Addr,Angle_Hight_Register_Addr);
	buf[1]=IIC2_Read_OneAddress(Slave_Addr,Angle_Low_Register_Addr);
	
	temp = (((u16)buf[0]<< 8) | buf[1]);
	temp=temp/4096*360;
  return temp;
}

float Encoder2_Action=0;

void Encoder2_update(void)
{
	static float encoder_Before=0;
	static float encoder_Now=0;
	float Different=0;

	encoder_Now=Get_Angle2();
	if(encoder_Now-encoder_Before<-200) Different=encoder_Now+360.0f-encoder_Before;
  else if(encoder_Now-encoder_Before>200) Different=encoder_Now-360.0f-encoder_Before;
	else Different=encoder_Now-encoder_Before;
	
	Encoder2_Action+=Different;
	
	encoder_Before=encoder_Now;
}

short offestx,offesty;


